import FCVector
import FCGenericCrowdEngine.Define as Define , Util

reload(Define)
reload(FCVector)
reload(Util)

class KdTree:
    def __init__( self, _Simulator ):
        self.mSimulator = _Simulator
        self.mAgentList = []
        self.mAgentTree = []
        self.mObstacleTree = None

    def fnBuildAgentTree( self ):
        if len( self.mAgentList ) < len( self.mSimulator.mAgentList ): 
            for _Idx in range( len(self.mAgentList) , len(self.mSimulator.mAgentList) ):
                self.mAgentList.append( self.mSimulator.mAgentList[_Idx] )

	        self.mAgentTree = []
	        for _Idx in xrange( 2 * len(self.mAgentList) - 1 ):
	            self.mAgentTree.append( Define.AgentTreeNode() )

        if 0 < len(self.mAgentList):
            self.fnBuildAgentTreeRecursive( 0, len(self.mAgentList), 0 ) 

    def fnBuildAgentTreeRecursive( self, _Begin, _End, _Node ):
        self.mAgentTree[_Node].Begin = _Begin
        self.mAgentTree[_Node].End = _End
        self.mAgentTree[_Node].MinX = self.mAgentList[_Begin].mPosition.x
        self.mAgentTree[_Node].MaxX = self.mAgentList[_Begin].mPosition.x
        self.mAgentTree[_Node].MinY = self.mAgentList[_Begin].mPosition.y
        self.mAgentTree[_Node].MaxY = self.mAgentList[_Begin].mPosition.y

        for _idx in range( _Begin + 1 , _End ):
            self.mAgentTree[_Node].MaxX = max( self.mAgentTree[_Node].MaxX , self.mAgentList[_idx].mPosition.x )
            self.mAgentTree[_Node].MinX = min( self.mAgentTree[_Node].MinX , self.mAgentList[_idx].mPosition.x )
            self.mAgentTree[_Node].MaxY = max( self.mAgentTree[_Node].MaxY , self.mAgentList[_idx].mPosition.y )
            self.mAgentTree[_Node].MinY = min( self.mAgentTree[_Node].MinY , self.mAgentList[_idx].mPosition.y )

        if (_End - _Begin) > Define.MAX_LEAF_SIZE:
            _IsVertical = (self.mAgentTree[_Node].MaxX - self.mAgentTree[_Node].MinX) > (self.mAgentTree[_Node].MaxY - self.mAgentTree[_Node].MinY)
            _SplitValue = 0.5 * (self.mAgentTree[_Node].MaxX + self.mAgentTree[_Node].MinX) \
                            if _IsVertical else \
                            0.5 * (self.mAgentTree[_Node].MaxY + self.mAgentTree[_Node].MinY)

            _Left = _Begin
            _Right = _End

            while _Left < _Right:
                while _Left < _Right and (self.mAgentList[_Left].mPosition.x if _IsVertical else self.mAgentList[_Left].mPosition.y) < _SplitValue :
                    _Left += 1

                while _Right > _Left and (self.mAgentList[_Right-1].mPosition.x if _IsVertical else self.mAgentList[_Right-1].mPosition.y) >= _SplitValue :
	                _Right -= 1

                if _Left < _Right :
                    self.mAgentList[_Left], self.mAgentList[_Right-1] = self.mAgentList[_Right-1], self.mAgentList[_Left]
                    
                    _Left += 1
                    _Right -= 1

            _LeftSize = _Left - _Begin

            if 0 == _LeftSize :
                _LeftSize += 1
                _Left += 1
                _Right += 1

            self.mAgentTree[_Node].Left = _Node + 1            
            self.mAgentTree[_Node].Right = _Node + 1 + ( 2 * _LeftSize - 1 )

            self.fnBuildAgentTreeRecursive( _Begin , _Left , self.mAgentTree[_Node].Left )
            self.fnBuildAgentTreeRecursive( _Left , _End , self.mAgentTree[_Node].Right )

    def fnBuildObstacleTree( self ):
        self.fnDeleteObstacleTree(self.mObstacleTree)
        _Obstacles = [Simulator.Obstacle() for x in range(0,len(self.mSimulator.mObstacleList))]
        for _idx in range(0,len(self.mSimulator.mObstacleList)):
            _Obstacles[_idx] = self.mSimulator.mObstacleList[_idx]                    
        self.mObstacleTree = self.fnBuildObstacleTreeRecursive(_Obstacles)

    def fnBuildObstacleTreeRecursive( self, _Obstacles ):        
        if len(_Obstacles)==0:	
            return False
        _pNode = transform.ObstacleTreeNode()
        _OptimalSplit = 0
        _MinLeft = len( _Obstacles )
        _MinRight = len( _Obstacles )	    

        for _NodeIdx in range( len(_Obstacles) ):
            _LeftSize = 0
            _RightSize = 0
            _pObstacleI1 = _Obstacles[_NodeIdx]
            _pObstacleI2 = _pObstacleI1.mNextObstacle

            for _SptIdx in range(len(_Obstacles)):
                if (_NodeIdx == _SptIdx):
                    continue

                _pObstacleJ1 = _Obstacles[_SptIdx]
                _pObstacleJ2 = _pObstacleJ1.mNextObstacle

                _J1LeftOfI = transform.leftof( _pObstacleI1.mPosition, _pObstacleI2.mPosition, _pObstacleJ1.mPosition )
                _J2LeftOfI = transform.leftof( _pObstacleI1.mPosition, _pObstacleI2.mPosition, _pObstacleJ2.mPosition )

                if (_J1LeftOfI >= -transform.RVO_EPSILON) and (_J2LeftOfI >= -transform.RVO_EPSILON):
                    _LeftSize += 1
                elif (_J1LeftOfI <= transform.RVO_EPSILON) and (_J2LeftOfI <= transform.RVO_EPSILON):
                    _RightSize += 1
                else:			
                    _LeftSize += 1
                    _RightSize += 1

                pair1 = [max(_LeftSize, _RightSize), min(_LeftSize, _RightSize)]
                pair2 = [max(_MinLeft, _MinRight), min(_MinLeft, _MinRight)]
                if pair1[0] >= pair2[0]:
                    break

            pair1 = [max(_LeftSize, _RightSize), min(_LeftSize, _RightSize)]
            pair2 = [max(_MinLeft, _MinRight), min(_MinLeft, _MinRight)]
            if( pair1<pair2):           
                _MinLeft = _LeftSize
                _MinRight = _RightSize
                _OptimalSplit = _NodeIdx

        _LeftObstacles = [Simulator.Obstacle() for idx in range(0,_MinLeft)]	
        _RightObstacles = [Simulator.Obstacle() for idx in range(0,_MinRight)]    

        _LeftCounter = 0
        _RightCounter = 0
        _Idx = _OptimalSplit

        _pObstacleI1 = _Obstacles[_Idx]
        _pObstacleI2 = _pObstacleI1.mNextObstacle

        for _Idx2 in range (0,len(_Obstacles)):            
            if (_Idx == _Idx2 ):
                continue

            _pObstacleJ1 = _Obstacles[_Idx2]
            _pObstacleJ2 = _pObstacleJ1.mNextObstacle

            j1LeftOfI = transform.leftof(_pObstacleI1.mPosition, _pObstacleI2.mPosition, _pObstacleJ1.mPosition)
            j2LeftOfI = transform.leftof(_pObstacleI1.mPosition, _pObstacleI2.mPosition, _pObstacleJ2.mPosition)
            
            if (j1LeftOfI >= -transform.RVO_EPSILON and j2LeftOfI >= -transform.RVO_EPSILON):                               
                _LeftObstacles[_LeftCounter] = _Obstacles[_Idx2]
                _LeftCounter+=1
            elif (j1LeftOfI <= transform.RVO_EPSILON and j2LeftOfI <= transform.RVO_EPSILON):
                _RightObstacles[_RightCounter] = _Obstacles[_Idx2]
                _RightCounter+=1
            else:            
                _Det1 = transform.determinant( _pObstacleI2.mPosition - _pObstacleI1.mPosition, _pObstacleJ1.mPosition - _pObstacleI1.mPosition )
                _Det2 = transform.determinant( _pObstacleI2.mPosition - _pObstacleI1.mPosition, _pObstacleJ1.mPosition - _pObstacleJ2.mPosition )
        	    
                _SplitPoint = _pObstacleJ1.mPosition + (_Det1 / _Det2) * (_pObstacleJ2.mPosition - _pObstacleJ1.mPosition)

                _pNewObstacle = Simulator.Obstacle()
                _pNewObstacle.mIdx = len(self.mSimulator.mObstacleList)
                _pNewObstacle.mNextObstacle = _pObstacleJ2
                _pNewObstacle.mPrevObstacle = _pObstacleJ1
                _pNewObstacle.mPosition = _SplitPoint
                _pNewObstacle.mNormDir = _pObstacleJ1.mNormDir
                _pNewObstacle.mIsConvex = True

                self.mSimulator.mObstacleList.append(_pNewObstacle)

                _pObstacleJ1.mNextObstacle = _pNewObstacle
                _pObstacleJ2.mPrevObstacle = _pNewObstacle

                if (j1LeftOfI > 0.0) :
                    _LeftObstacles[_LeftCounter] = _pObstacleJ1
                    _LeftCounter+=1
                    _RightObstacles[_RightCounter] = _pNewObstacle
                    _RightCounter+=1
                else :
                    _LeftObstacles[_LeftCounter] = _pObstacleJ1
                    _LeftCounter+=1
                    _RightObstacles[_RightCounter] = _pNewObstacle
                    _RightCounter+=1

        _pNode.pObstacle = _pObstacleI1
        _pNode.pLeft = self.fnBuildObstacleTreeRecursive( _LeftObstacles )
        _pNode.pRight = self.fnBuildObstacleTreeRecursive( _RightObstacles )

        return _pNode

    def fnComputeAgentNeighbors( self, _Agent, _RangeSq ):
        return self.fnQueryAgentTreeRecursive(_Agent,_RangeSq, 0 )

    def fnComputeObstacleNeighbors( self, _Agent, _RangeSq ):
        self.fnQueryObstacleTreeRecursive( _Agent, _RangeSq, self.mObstacleTree )

    def fnDeleteObstacleTree( self, _Node ):
        if None != _Node and 0 != _Node:
            self.fnDeleteObstacleTree(_Node.pLeft)
            self.fnDeleteObstacleTree(_Node.pRight)            

    def fnQueryAgentTreeRecursive( self, _pAgent, _RangeSq, _Node ):
        if (self.mAgentTree[_Node].End - self.mAgentTree[_Node].Begin) <= Define.MAX_LEAF_SIZE:
            for _idx in range(self.mAgentTree[_Node].Begin,self.mAgentTree[_Node].End):
                _pAgent.fnInsertAgent(self.mAgentList[_idx], _RangeSq)
        else:
            _DistSqLeft = \
                Util.square( max( 0.0, self.mAgentTree[self.mAgentTree[_Node].Left].MinX - _pAgent.mPosition.x ) ) + \
                Util.square( max( 0.0, _pAgent.mPosition.x - self.mAgentTree[self.mAgentTree[_Node].Left].MaxX ) ) + \
                Util.square( max( 0.0, self.mAgentTree[self.mAgentTree[_Node].Left].MinY - _pAgent.mPosition.y ) ) + \
                Util.square( max( 0.0, _pAgent.mPosition.y - self.mAgentTree[self.mAgentTree[_Node].Left].MaxY ) )

            _DistSqRight = \
                Util.square( max( 0.0, self.mAgentTree[self.mAgentTree[_Node].Right].MinX - _pAgent.mPosition.x ) ) + \
                Util.square( max( 0.0, _pAgent.mPosition.x - self.mAgentTree[self.mAgentTree[_Node].Right].MaxX ) ) + \
                Util.square( max( 0.0, self.mAgentTree[self.mAgentTree[_Node].Right].MinY - _pAgent.mPosition.y ) ) + \
                Util.square( max( 0.0, _pAgent.mPosition.y - self.mAgentTree[self.mAgentTree[_Node].Right].MaxY ) )

            if( _DistSqLeft < _DistSqRight ):
                if( _DistSqLeft < _RangeSq ):
                    _RangeSq = self.fnQueryAgentTreeRecursive( _pAgent, _RangeSq, self.mAgentTree[_Node].Left )    	            
                    if( _DistSqRight < _RangeSq ):
                        _RangeSq = self.fnQueryAgentTreeRecursive( _pAgent, _RangeSq, self.mAgentTree[_Node].Right )
            else:
                if( _DistSqRight < _RangeSq ):
                    _RangeSq = self.fnQueryAgentTreeRecursive( _pAgent, _RangeSq, self.mAgentTree[_Node].Right)
                    if( _DistSqLeft < _RangeSq ):
                        _RangeSq = self.fnQueryAgentTreeRecursive( _pAgent, _RangeSq, self.mAgentTree[_Node].Left)
                        
            return _RangeSq

    def fnQueryObstacleTreeRecursive( self, _pAgent, _RangeSq, _pNode ):
        if( None == _pNode or 0 == _pNode):
            return

        _pObstacle1 = _pNode.pObstacle
        _pObstacle2 = _pObstacle1.mNextObstacle
        _AgentLeftOfLine = transform.leftof( _pObstacle1.mPosition, _pObstacle2.mPosition, _pAgent.mPosition )     
        
        if _AgentLeftOfLine>=0.0:
            self.fnQueryObstacleTreeRecursive( _pAgent, _RangeSq, ( _pNode.pLeft))
        else:
            self.fnQueryObstacleTreeRecursive( _pAgent, _RangeSq, ( _pNode.pRight))
        
        _DistSqLine = Util.square(_AgentLeftOfLine) / (_pObstacle2.mPosition - _pObstacle1.mPosition).lengthsq()

        if( _DistSqLine < _RangeSq ):
            if(  0.0 > _AgentLeftOfLine ):
                _pAgent.fnInsertObstacle( _pNode.pObstacle, _RangeSq )
                
            if _AgentLeftOfLine >= 0.0:
                self.fnQueryObstacleTreeRecursive( _pAgent, _RangeSq, ( _pNode.pRight))
            else:
                self.fnQueryObstacleTreeRecursive( _pAgent, _RangeSq, ( _pNode.pLeft))

    def fnQueryVisibility( self, _Q1, _Q2, _Radius ):
        self.fnQueryVisibilityRecursive( _Q1,_Q2,_Radius,self.mObstacleTree )

    def fnQueryVisibilityRecursive( _Q1, _Q2, _Radius, _Node ):
        if( None == _pNode ):
            return True

        _pObstacle1 = _pNode.pObstacle
        _pObstacle2 = _pObstacle1.mNextObstacle
        _Q1LeftOfI = transform.leftof( _pObstacle1.mPosition, _pObstacle2.mPosition, _Q1 )
        _Q2LeftOfI = transform.leftof( _pObstacle1.mPosition, _pObstacle2.mPosition, _Q2 )
        _InvLengthI = 1.0 / (_pObstacle2.mPosition - _pObstacle1.mPosition).lengthsq()

        if( _Q1LeftOfI >= 0.0 and _Q2LeftOfI >= 0.0 ):
            return self.fnQueryVisibilityRecursive( _Q1, _Q2, _Radius, _pNode.pLeft ) \
                and ( (Util.square(_Q1LeftOfI) * _InvLengthI >= Util.square(_Radius) and Util.square(_Q2LeftOfI) * _InvLengthI >= Util.square(_Radius) ) \
	                or self.fnQueryVisibilityRecursive( _Q1, _Q2, _Radius, _pNode.pRight ) )

        elif( _Q1LeftOfI <= 0.0 and _Q2LeftOfI <= 0.0 ):
            return self.fnQueryVisibilityRecursive( _Q1, _Q2, _Radius, _pNode.pRight ) \
                and ( (Util.square(_Q1LeftOfI) * _InvLengthI >= Util.square(_Radius) and Util.square(_Q2LeftOfI) * _InvLengthI >= tansform.square(_Radius) ) \
	            or self.fnQueryVisibilityRecursive( _Q1, _Q2, _Radius, _pNode.pLeft ) )

        elif( _Q1LeftOfI >= 0.0 and _Q2LeftOfI <= 0.0 ):   
            return self.fnQueryVisibilityRecursive( _Q1, _Q2, _Radius, _pNode.pLeft ) \
                and self.fnQueryVisibilityRecursive( _Q1, _Q2, _Radius, _pNode.pRight )

        else:
            _Point1LeftOfQ = transform.leftof( _Q1, _Q2, _pObstacle1.mPosition )
            _Point2LeftOfQ = transform.leftof( _Q1, _Q2, _pObstacle2.mPosition )
            _InvLengthQ = 1.0 / (_Q2 - _Q1).lengthsq()

            return ( _Point1LeftOfQ * _Point2LeftOfQ >= 0.0 \
                and Util.square(_Point1LeftOfQ) * _InvLengthQ > Util.square(_Radius) \
                and Util.square(_Point2LeftOfQ) * _InvLengthQ > Util.square(_Radius) \
                and self.fnQueryVisibilityRecursive( _Q1, _Q2, _Radius, _pNode.pLeft ) \
                and self.fnQueryVisibilityRecursive( _Q1, _Q2, _Radius, _pNode.pRight ) )
